#pragma once

#include <typedef.h>

// 定义系统状态矩阵类型
typedef Eigen::Matrix<double, 2, 1> StateMat;
// 定义误差协方差矩阵类型
typedef Eigen::Matrix<double, 2, 2> Covariance;
// 定义状态转移矩阵类型
typedef Eigen::Matrix<double, 2, 2> StateTransition;
// 定义过程噪声协方差矩阵类型
typedef Eigen::Matrix<double, 2, 2> ProcessNoise;
// 定义观测向量类型
typedef Eigen::Matrix<double, 2, 1> Measurement;
// 定义观测噪声协方差矩阵类型
typedef Eigen::Matrix<double, 2, 2> MeasurementNoise;
// 定义观测矩阵类型
typedef Eigen::Matrix<double, 2, 2> Observation;

class KfPosVel
{
public:
	KfPosVel();
	~KfPosVel();
	void SetParam(double dt, double s, double r);
	void Update(double accel, double position, double velocity, double& est_pos, double& est_vel);

private:
	// 定义状态向量 [位置, 速度]
	StateMat state;
	// F：状态转移矩阵
	Eigen::Matrix<double, 2, 2> F;
	// B：输入控制矩阵
	Eigen::Matrix<double, 2, 1> B;
	// 定义误差协方差矩阵
	Covariance P;
	// Q：过程噪声协方差矩阵
	ProcessNoise Q;
	// 观测矩阵
	Observation H;
	// 观测向量
	Measurement measurement;
	// 观测噪声协方差
	MeasurementNoise R;
	// 卡尔曼增益
	Eigen::Matrix<double, 2, 2> K;
};